/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>
#include <ceres/ceres.h>
#include "common.h"
#include "SnavelyReprojectionError.h"

using namespace std;

void SolveBA(BALProblem &bal_problem);

int main(int argc, char** argv){
    if(argc!=2){
        cout<<"usage: bundle_adjustment_ceres bal_data.txt"<<endl;
        return 1;
    }
 
    BALProblem bal_problem(argv[1]);
    bal_problem.Normalize();
    bal_problem.Perturb(0.1,0.5,0.5,);
    bal_problem.WriteToPLYFile("initial.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("final.ply");
    
    return 0;
}
    
void SolveBA(BALProblem& bal_problem)
{
 const int point_block_size=bal_problem.point_block_size();
 const int camera_block_size=bal_problem.camera_block_size();
 double *points = bal_problem.mutable_points();
 double *cameras =bal_problem.mutable_cameras();
 
 //Observation is 2*num_observations long array num_observations
 //[u_1,u_2,....u_n] where each u_i is two dimensional, the x
 //and y position of the Observation.
const double *observations =bal_problem.observations();
ceres::Problem problem;

for(int i=0;i<bal_problem.num_observations();++i)
{
    ceres::CostFunction *Cost_function;
    
    //each residual block takes a point and a camera as input
    //and output a 2 dimensional residual
    Cost_function = SnavelyReprojectionError::Create(observations[2*i+0],observations[2*i+1]);
    
     //if enable use huber's loss function 
    ceres::LossFunction *loss_function=new ceres::HuberLoss(1.0);
    
    //Each observation corressponds to a pair of a camera and a point 
    //which are identified by camera_index()[i] and point_index()[i]
    // respectively.
    double *camera=camera+camera_block_size*bal_problem.camera_index()[i];
    double *point=point+point_block_size*bal_problem.point_index()[i];
    
    problem.AddResidualBlock(Cost_function,loss_function,camera,point);
}

//show some information here ...
std::cout<<"bal problem file loaded..."<<std::endl;
std::cout<<"bal problem have "<<bal_problem.num_cameras()<<" cameras and"
    <<bal_problem.num_points()<<"points. "<<std::endl;
    
std::cout<<"Forming "<<bal_problem.num_observations()<<" observations. "<<std::endl;

std::cout<<"Solving ceres BA ..."<<endl;
ceres::Solver::Options options;
options.linear_solver_type=ceres::LinearSolverType::SPARSE_SCHUR;
options.minimizer_progress_to_stdout=true;
ceres::Solver::Summary summary;
ceres::Solver(options, &problem, &summary);
std::cout<<summary.FullReport()<<"\n";
}

